import ia.util.LectorImagenes;

import java.util.*;

public class Monticulo {
	private PriorityQueue<Nodo> monticulo;
	private long width, height;

	public Monticulo() {
		this.monticulo = new PriorityQueue<Nodo>();
		this.width = 0;
		this.height = 0;
	}

	public void add(Nodo n) {
		try {
			monticulo.add(n);
		} catch (java.lang.OutOfMemoryError e) {
			System.out.println("Monticulo: " + monticulo.toString()
					+ "\nTamano: " + size());
			System.out.println(e.toString());
		}
	}

	public double size() {
		return monticulo.size();
	}

	@Override
	public String toString() {
		return monticulo.toString();
	}
	
	/*
	Función ASCENSIÓN-COLINAS (problema)
		devuelve estado que es un máximo local
		entradas: problema, un problema
		variables locales:	actual, un nodo
							vecino, un nodo
	actual ← HACER-NODO (ESTADO-INICIAL[problema])
	bucle hacer
		vecino ← sucesor de valor más alto de actual
		si VALOR[vecino] ≤ VALOR[actual] entonces devolver ESTADO[actual]
		actual ← vecino
	*/
	
	public Camino ascensoColinas(String ruta, Camino sol){
		Camino actual=sol, vecino, res=null;
		LectorImagenes li=new LectorImagenes();
		li.getMapaGS(ruta);
		int width = li.getImagen().getWidth(null);
		int height = li.getImagen().getHeight(null);
		boolean sigue=true;
		Random rd = new Random();
		int posX, posY, max=20/*Math.max(width, height)*/, r;
		Posicion p;
		while(sigue){
			posX=rd.nextInt(width);
			posY=rd.nextInt(height);
			p=new Posicion(posX,posY);
			r=rd.nextInt(max/2);
			System.out.println("Eliminando a partir de la posicion " + p.toString() + " con un radio " + r);
			actual.eliminaCirculo(p, (long)r);
			vecino=restaurarSolucion(actual, ruta);
			if (vecino.getC() <= actual.getC()){
				res=actual;
				sigue=false;
			}
			actual=vecino;
		}
		return res;
	}
	
	
	/*
	 * Función TEMPLE-SIMULADO (problema, esquema)
			devuelve estado solución
			entradas:	problema, un problema
						esquema, una aplicación desde el tiempo a Temperatura
			variables locales:	actual, un nodo
								siguiente, un nodo
								T, una “Temperatura” probabilidad de paso abajo
		actual ← HACER-NODO (ESTADO-INICIAL[problema])
		para t ← 1 a infinito hacer
			T ← esquema[t]
			si T=0 entonces devolver actual
			siguiente ← un sucesor seleccionado aleatoriamente de actual
			∆E ← VALOR[siguiente] – VALOR[actual]
			si ∆E > 0 entonces actual ← siguiente
			en caso contrario actual ← siguiente sólo con probabilidad e^∆E/T
	 */
	
	public Camino templeSimulado(String ruta, Camino sol, int T0){
		Camino actual=sol, vecino, aux, res=null;
		LectorImagenes li=new LectorImagenes();
		li.getMapaGS(ruta);
		int width = li.getImagen().getWidth(null);
		int height = li.getImagen().getHeight(null);
		Random rd = new Random();
		int T, posX, posY, max=20 /*Math.max(width, height)*/,r;
		double AE;
		Posicion p;
		for (int t=1; t <= T0 && res==null ; t++){
			T = T0 - t;
			if (T==0)
				res=actual;
			else{
				posX=rd.nextInt(width);
				posY=rd.nextInt(height);
				p=new Posicion(posX, posY);
				r=rd.nextInt(max/2);
				aux=actual;
				System.out.println("Eliminando a partir de la posicion " + p.toString() + " con un radio " + r);
				aux.eliminaCirculo(p, (long)r);
				vecino=restaurarSolucion(aux, ruta);
				AE=vecino.getC() - actual.getC();
				if (AE > 0){
					actual=vecino;
					System.out.println(actual.toString3());
				}
				else{
					if(rd.nextDouble() < Math.pow(Math.E, AE/T))
						actual=vecino;
				}
			}
			//System.out.println(actual.toString());
		}
		return res;
	}
	
	
	public double val(Camino solucion, String ruta){
		LectorImagenes li = new LectorImagenes();
		int mapa[][] = li.getMapaGS(ruta);
		
		
		double valor=0;
		int x,y;
		for (int i=0;i<solucion.getCamino().size();i++){
			x=(int)solucion.getCamino().get(i).getPos().getX();
			y=(int)solucion.getCamino().get(i).getPos().getY();
			valor+=calcularCoste(solucion.getCamino().get(i).getPieza(),mapa[x][y]);
		}
		return valor;
	}

	/* Restaura el camino cuando hay un corte entre las posiciones cortei y cortef */
	public Camino restaurarCamino(Camino c, Posicion cortei, Posicion cortef, String ruta){
		System.out.println("Restaurando entre los puntos " + cortei.toString() + " y " + cortef.toString());
		Camino aux=busqueda(ruta, cortei,cortef, "A*", 0, 0, 0, "manhatan");
		/* System.out.println(aux.toString());*/	
		int i=0;
		while (!c.getCamino().get(i++).getPos().equals(cortei));
		c.getCamino().remove(i-1);
		/* Nos posicionamos en el nuevo inicio */
		i--;
		for (int j=0;j<aux.getCamino().size();j++,i++){
			aux.getCamino().get(j).setPadre(c.getCamino().get(i-1));
			c.getCamino().add(i,aux.getCamino().get(j));
		}
		c.getCamino().get(i).setPadre(c.getCamino().get(i-1));
		return c;
	}
	
	/* Busca en el camino el primer corte y devuelve la posicion en la lista del ultimo nodo antes del corte */
	public int busquedaNodoCorte(Camino c){
		int inodo=-1;
		for (int i=1; i<c.getCamino().size(); i++)
			if (numeroEnlaces(c, c.getCamino().get(i))==1)
				return i;
		return inodo;
	}
	
	/* Toma la solucion de entrada como ordenada y restaura los caminos cuando encuentra cortes */
	public Camino restaurarSolucion(Camino c, String ruta){
		int inodo=busquedaNodoCorte(c);
		while (inodo!=c.getCamino().size()-1 && inodo!=-1){
			//System.out.println(c.getCamino().get(inodo) + " - " + c.getCamino().get(inodo+1)); 
			c=restaurarCamino(c,c.getCamino().get((int)inodo).getPos(), c.getCamino().get((int)inodo+1).getPos(), ruta);
			inodo=busquedaNodoCorte(c);
		}
		c.setC(val(c,ruta));
		return c;
	}
	
	/* Se devuelve el numero de enlaces que tiene un nodo. dir sirve para indicar la direccion en caso de que solo
	 * tenga un vecino.
	 */
	private int numeroVecinos(Camino solucion, Nodo actual){
		int n=0,dir=0;
		long x,y,xi,yi;
		Nodo inode;
		x=actual.getPos().getX();
		y=actual.getPos().getY();
		for (int i=0;i<solucion.getCamino().size();i++){
			inode=solucion.getCamino().get(i);
			if (!inode.equals(actual)){
				xi=inode.getPos().getX();
				yi=inode.getPos().getY();
				if(x==xi && Math.abs(y-yi)==1){
					n++;
					dir=-1;
				}
				if (Math.abs(x-xi)==1 && y==yi){
					n++;
					dir=1;
				}
			}
		}
		return n*dir;
	}
	
	
	/* Se devuelve el numero de vecinos que tiene un nodo.
	 */
	private int numeroEnlaces(Camino solucion, Nodo actual){
		int n=0;
		for (int i=0;i<solucion.getCamino().size();i++){
			if (i==0){
				if(actual.getPadre().equals(solucion.getCamino().get(i))) n++;
			}else{
				if (actual.equals(solucion.getCamino().get(i).getPadre()) || 
					actual.getPadre().equals(solucion.getCamino().get(i)))
					n++;
			}
		}
		return n;
	}
	
	/* Se calcula la pieza que se ha de poner en el nodo a para llegar al b en caso de que
	 * sean vecinos.
	 */
	private int calcularPieza(Nodo a, Nodo b){
		int pieza=0;
		long xa,ya,xb,yb;
		xa=a.getPos().getX();
		ya=a.getPos().getY();
		xb=b.getPos().getX();
		yb=b.getPos().getY();
		
		
		if (xa==xb && Math.abs(ya-yb)==1){
			if(a.getDireccion()==1)
				pieza=3;
			else if (a.getDireccion()==-1)
				pieza=1;
			else
				pieza=-1;
		}
		if (Math.abs(xa-xb)==1 && ya==yb){
			if(a.getDireccion()==1)
				pieza=1;
			else if (a.getDireccion()==-1)
				pieza=3;
			else
				pieza=-1;
		}
		//System.out.println(a.toString() + " - " + b.toString() + " - " + Math.abs(xa-xb) + " - " + Math.abs(ya-yb) + " - " + pieza);
		return pieza;
	}
	
	/* Coloca la solucion dada por el conjunto de posiciones desordenadas */
	private Camino colocarSolucion(Camino sol){
		Camino solFinal=new Camino();
		Nodo actual;
		int pieza,dir;
		for (int i=0;i<sol.getCamino().size() && solFinal.getCamino().isEmpty();i++){
			dir=numeroVecinos(sol,sol.getCamino().get(i));
			if(Math.abs(dir)==1){
				sol.getCamino().get(i).setPieza(1);
				sol.getCamino().get(i).setDireccion(dir);
				solFinal.getCamino().add(sol.getCamino().get(i));
				sol.getCamino().remove(i);
			}
		}
		/*
		for (int i=0;i<sol.getCamino().size();i++){
			System.out.println(sol.getCamino().get(i).toString());
		}
		*/
		actual=solFinal.getCamino().get(0);
		//System.out.println("\nPosicion: " + actual.toString() + "\nDireccion: " + actual.getDireccion() + "\nTamaño: " + sol.getCamino().size());
		while(!sol.getCamino().isEmpty()){
			for (int i=0;i<sol.getCamino().size();i++){
				pieza = calcularPieza(actual,sol.getCamino().get(i));
				if (pieza > 0){
					sol.getCamino().get(i).setPadre(actual);
					if (pieza==3) sol.getCamino().get(i).setDireccion(actual.getDireccion()*(-1));
					else sol.getCamino().get(i).setDireccion(actual.getDireccion());
					sol.getCamino().get(i).setPieza(pieza);
					solFinal.getCamino().add(sol.getCamino().get(i));
					actual=sol.getCamino().remove(i);
					//System.out.println("Pieza: " + pieza + "\nDireccion: " + actual.getDireccion() + "\nPosicion: " + actual.toString() + "\nTamaño: " + sol.getCamino().size());
				}
			}
		}
		
		solFinal.setC(0.0);
		for (int i=0;i<solFinal.getCamino().size();i++)
			solFinal.setC(solFinal.getC()+solFinal.getCamino().get(i).getCoste());
		
		
		solFinal.setM(0.0);
		solFinal.setT(0.0);
		return solFinal;
	}
	
	/* Obtiene una solucion a partir de una imagen con el camino dibujado */
	public Camino getSolucion(String ruta){
		Camino solucion = new Camino();
		LectorImagenes li = new LectorImagenes();
		int mapa[][] = li.getSolucion(ruta);
		int width = li.getImagen().getWidth(null);
		int height = li.getImagen().getHeight(null);
		/* Escribe el mapa 
		for (int i=0;i<height;i++){
			for (int j=0;j<width;j++)
				System.out.print(mapa[j][i] + "\t");
			System.out.println();
		}
		*/
		for (int i=0;i<height;i++)
			for (int j=0;j<width;j++)
				if (mapa[i][j]==1)
					solucion.getCamino().add(new Nodo(new Posicion(i,j)));
		
		
		return colocarSolucion(solucion);
	}
	
	private Camino construirSolucion(Nodo n){
		Camino res=new Camino();
		Nodo aux=n;
		while(aux.getPadre()!=null){
			res.getCamino().add(0, aux);
			aux=aux.getPadre();
		}
		res.getCamino().add(0, aux);
		return res;
	}
	
	private Nodo seleccionarCandidatoA(int profundidad){
		if (monticulo.peek() != null)	/* Comprobamos si la cabeza es nula */
			if (monticulo.peek().getProfundidad()==profundidad)	/* Si la cabeza tiene la profundidad actual */
				return monticulo.poll();	/* La devolvemos */
			else return new Nodo(new Posicion(-1,-1),0,0,0,0,null);	/* Si no, devolvemos un nodo error */
		else
			return null;	/* Si no, devolvemos null */ 
	}

	private Nodo seleccionarCandidatoP() {
		return monticulo.poll();
	}
	
	private double calcularCoste(int ng, int pieza) {
		double res = 0, multCost = 0;
		switch (pieza) {
		case 1:
			multCost = 5;
			break;
		case 2:
			multCost = 3;
			break;
		case 3:
			multCost = 6;
			break;
		case 4:
			multCost = 6;
			break;
		default:
			multCost = 0;
		}
		if (ng == 0)
			multCost *= 100;
		if (pieza == 3 || pieza == 4)
			res = ng * 3 * multCost;
		else
			res = ng * 1 * multCost;

		return res;
	}
	
	private double calcularCosteheuristica(String heuristica, Posicion posActual, Posicion posFin){
		double res=0;
		if (heuristica.equals("euclidea")){
			res=Math.sqrt(Math.pow(posActual.getX()-posFin.getX(), 2)) + Math.sqrt(Math.pow(posActual.getY()-posFin.getY(), 2));
			res*=calcularCoste(1,1);
		}else if (heuristica.equals("manhattan")){
			res=Math.abs(posActual.getX()-posFin.getX()) + Math.abs(posActual.getY()-posFin.getY());
			res*=calcularCoste(1,1);
		}
		return res;
	}
	
	/*
	 * Metodo general de Busqueda mapa: nombre del fichero de la imagen. PosIni:
	 * (x,y) del comienzo del camino. PosFin(x,y) del destino. Estrategia:
	 * Anchura, Costo Uniforme, Profundidad, ProdundidadAcotada,
	 * ProfundidadIterava. Solo si hay profundidad acotada entonces: cota es el
	 * valor maximo de la cota. Solo si hay profundidad iterativa entonces: cota
	 * para la primera iteracion aumentocota para el incremento de cota en
	 * distintas iteraciones numItera el numero de iteraciones maximo.
	 */

	public Camino busqueda(String rutaMapa, Posicion posIni, Posicion posFin,
			String estrategia, long cota, long aumentocota, int numItera, String heuristica) {
		Camino res = new Camino();
		/*
		 * creamos un lector de imagenes y tomamos la matriz de valores de
		 * grises
		 */
		LectorImagenes li = new LectorImagenes();
		int mapa[][] = li.getMapaGS(rutaMapa);
		width = li.getImagen().getWidth(null);
		height = li.getImagen().getHeight(null);

		/*
		 * Tomamos el primer valor de tiempo para comprobar cuanto tarda en
		 * conseguir la solucion
		 */
		double itime, ftime;
		itime = System.currentTimeMillis();
		if(posIni.equals(posFin)){
			res.getCamino().add(new Nodo(posIni, 1, calcularCoste(mapa[(int)posIni.getX()][(int)posIni.getY()],1) , 0, 0, null));
		}else {
		/* segun la estrategia escogida hacemos una busqueda especializada */
		if (estrategia.equals("profundidad"))
				res = busquedaProfundidad(mapa, posIni, posFin);
			else if (estrategia.equals("profundidadAcotada"))
				res = busquedaProfundidadAcotada(mapa, posIni, posFin, cota);
			else if (estrategia.equals("profundidadIterativa"))
				res = busquedaProfundidadIterativa(mapa, posIni, posFin, cota,
						aumentocota, numItera);
			else if (estrategia.equals("anchura"))
				res = busquedaAnchura(mapa, posIni, posFin);
			else if (estrategia.equals("costoUniforme"))
				res = busquedaCostoUniforme(mapa, posIni, posFin);
			else if (estrategia.equals("A*"))
				res = busquedaA(heuristica, mapa, posIni, posFin);
		}
		/* Tomamos el valor final del tiempo y lo guardamos en la solucion */
		ftime = System.currentTimeMillis() - itime;
		res.setT(ftime/1000);

		return res;
	}

	private Camino busquedaA(String heuristica, int[][] mapa, Posicion posIni, Posicion posFin){
		Camino resOptimo = new Camino();
		Camino res = new Camino();
		boolean solucionEncontrada = false;

		/* Inicializamos la solucion optima con el peor valor */
		resOptimo.setC(9999999.0);
		/* Anadimos el primer nodo como candidato */
		Nodo n = new Nodo(posIni, 0, 0, 0, 0, null);
		n.setDireccion(1);	/* Escogemos horizontal como la primera direccion */
		monticulo.add(n);
		
		/* Definimos el nodo error. Este indicara que quedan candidatos, pero que no son
		 * de la misma profundidad que la actual y tendremos que aumentarla. Es decir,
		 * se han comprobado todos los nodos de un nivel.
		 */
		Nodo error=new Nodo(new Posicion(-1,-1),0,0,0,0,null);
		
		Nodo [] copiasCandidato = new Nodo[4];
		int profundidadActual = 0;
		double maxNodos = 0;
		/* Comenzamos el bucle de busqueda */
		while (!solucionEncontrada) {
			n = seleccionarCandidatoP();
			if (n != null) { /* Hay candidatos */
				if (n.equals(error)){ /* Hemos comprobado todos los candidatos de un nivel */
					profundidadActual++; /* Pasamos al nivel siguiente */
				}else{
					if (n.getPos().equals(posFin)) { /* Si es el nodo objetivo */
						/* Ponemos una pieza de avanzar simple y hemos encontrado una solucion */
						n.setPieza(1);
						/*if (n.getPadre()==null) System.out.println(n.toString());*/
						n.setCoste(n.getPadre().getCoste() + calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], 1));
						res=construirSolucion(n);
						n.getVisitados().add(posFin);
						/* Calculamos el coste de la solucion y el numero maximo de nodos almacenados */
						res.setC(n.getCoste());
						res.setM(maxNodos);
						System.out.println(res.toString3());
						/* Comprobamos si es la mejor solucion */
						if (res.getC() < resOptimo.getC())
							resOptimo=res;
					} else { /* Si no es el nodo objetivo */
						
						for (int i=1;i<5;i++){ /* Probamos todas las piezas */
							/* Hacemos una copia del candidato para que no se sobrescriban las piezas */
							copiasCandidato[i-1]= new Nodo(n.getPos(),0,0,n.getF(),n.getProfundidad(),n.getPadre());
							copiasCandidato[i-1].setDireccion(n.getDireccion());
							if(copiasCandidato[i-1].getPadre() != null){  /* Pixel intermedio */
								copiasCandidato[i-1].setPieza(i);
								copiasCandidato[i-1].setCoste(copiasCandidato[i-1].getPadre().getCoste()+ calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
							} else { /* Pixel inicial */
								if(i==1 || i==2){
									copiasCandidato[i-1].setPieza(i);
									copiasCandidato[i-1].setCoste(calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
								}
							}
							copiasCandidato[i-1].getVisitados().add(copiasCandidato[i-1].getPos());
							/* Si el coste aun es menor que el de la mejor solucion */
							if (copiasCandidato[i-1].getCoste()<resOptimo.getC())
								generarCandidatos(copiasCandidato[i-1], copiasCandidato[i-1].getDireccion(),profundidadActual,
										copiasCandidato[i-1].getCoste()+calcularCosteheuristica(heuristica,copiasCandidato[i-1].getPos(), posFin));
										/* Como es busqueda A* le pasamos el coste acumulado hasta este nodo mas el coste
										 * de la distancia hasta el objetivo segun la heuristica */
							else /* Si no */
								/* Se abandona este camino*/ ;
						}
						if (maxNodos < monticulo.size())
							maxNodos = monticulo.size();
					}
				}
			}else{ /* Si no hay candidatos */
				solucionEncontrada=true;
				if (!resOptimo.getCamino().isEmpty())
					System.out.println("Se ha encontrado Solucion");
				else
					System.out.println("No se ha encontrado Solucion");
			}
		}

		return resOptimo;
	}
	
	private Camino busquedaProfundidad(int[][] mapa, Posicion posIni,
			Posicion posFin) {
		Camino res = new Camino();
		boolean solucionEncontrada = false;

		/* Anadimos el primer nodo como candidato */
		Nodo n = new Nodo(posIni, 0, 0, 0, 0, null);
		monticulo.add(n);

		int profundidadActual = 0;
		double maxNodos = 0;
		int direccion = 1; /* 1 = horizontal, -1 = vertical */
		/* Comenzamos el bucle de busqueda */
		while (!solucionEncontrada) {
			n = seleccionarCandidatoP();
			if (n != null) { /* Hay candidatos */
				if (n.getPos().equals(posFin)) { /* Si es el nodo objetivo */
					/* Ponemos una pieza de avanzar simple y hemos encontrado solucion */
					n.setPieza(1);
					n.setCoste(n.getPadre().getCoste() + calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], 1));
					n.getVisitados().add(posFin);
					res.getCamino().add(n);
					/*
					 * Calculamos el coste de la solucion y el numero maximo de
					 * nodos almacenados
					 */
					res.setC(n.getCoste());
					res.setM(maxNodos);
					solucionEncontrada = true;
				} else { /* Si no es el nodo objetivo */
					
					if (n.getPiezasProbadas() <= 4) { /* Comprobamos si quedan piezas por probar En orden: recta
					 								   * simple, recta doble, curva a izqda curva a dcha */	
						if (n.getPadre() != null) { /* Pixel intermedio */
							n.setPieza(n.getPiezasProbadas());
							n.setCoste(n.getPadre().getCoste()+ calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], n.getPieza()));
						} else { /* Pixel inicial */
							if (n.getPiezasProbadas() == 1 || n.getPiezasProbadas() == 2) {
								n.setPieza(n.getPiezasProbadas());
								n.setCoste(calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], n.getPieza()));
							}
						}
						
						n.setPiezasProbadas(n.getPiezasProbadas() + 1); /* piezasProbadas++ */
						n.getVisitados().add(n.getPos());
						res.getCamino().add(n);
					
						generarCandidatos(n, direccion, profundidadActual, profundidadActual+1);
						profundidadActual++;
						
						if (maxNodos < monticulo.size())
							maxNodos = monticulo.size();
						
						if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1; /* Si hay curva se cambia de direccion */
						
					} else { /* Si hemos probado todas las piezas, volvemos atras */
						if (profundidadActual>=2){
							/* Quitamos la ultima pieza colocada y no valida */
							profundidadActual--;
							n=res.getCamino().get(profundidadActual);
							if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
							res.getCamino().remove(profundidadActual);
							/* Quitamos el padre y lo volvemos a incluir como nodo objetivo para que intente seguir otro camino */
							profundidadActual--;
							n=res.getCamino().get(profundidadActual);
							res.getCamino().remove(profundidadActual);
							n.setCoste(0);
							if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
							n.setPieza(0);
							monticulo.add(n);
						}
					}
				}
			}else{ /* Si no hay candidatos */
				if(profundidadActual>=1){
					/* Quitamos el padre y lo volvemos a incluir como nodo objetivo para que intente seguir otro camino */
					profundidadActual--;
					n=res.getCamino().get(profundidadActual);
					res.getCamino().remove(profundidadActual);
					n.setCoste(0);
					if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
					n.setPieza(0);
					monticulo.add(n);
				}
			}
		}

		return res;
	}
	
	private Camino busquedaProfundidadAcotada(int[][] mapa, Posicion posIni,
			Posicion posFin, long cota) {
		Camino res = new Camino();
		boolean solucionEncontrada = false;

		/* Anadimos el primer nodo como candidato */
		Nodo n = new Nodo(posIni, 0, 0, 0, 0, null);
		monticulo.add(n);

		int profundidadActual = 0;
		double maxNodos = 0;
		int direccion = 1; /* 1 = horizontal, -1 = vertical */
		/* Comenzamos el bucle de busqueda */
		while (!solucionEncontrada) {
			n = seleccionarCandidatoP();
			
			if (n != null) { /* Hay candidatos */
				if (n.getPos().equals(posFin)) { /* Si es el nodo objetivo */
					/* Ponemos una pieza de avanzar simple y hemos encontrado solucion */
					n.setPieza(1);
					n.setCoste(n.getPadre().getCoste() + calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], 1));
					n.getVisitados().add(posFin);
					res.getCamino().add(n);
					/*
					 * Calculamos el coste de la solucion y el numero maximo de
					 * nodos almacenados
					 */
					res.setC(n.getCoste());
					res.setM(maxNodos);
					solucionEncontrada = true;
				} else { /* Si no es el nodo objetivo */
					
					if (n.getPiezasProbadas() <= 4) { /* Comprobamos si quedan piezas por probar En orden: recta
					 								   * simple, recta doble, curva a izqda curva a dcha */	
						if (n.getPadre() != null) { /* Pixel intermedio */
							n.setPieza(n.getPiezasProbadas());
							n.setCoste(n.getPadre().getCoste()+ calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], n.getPieza()));
						} else { /* Pixel inicial */
							if (n.getPiezasProbadas() == 1 || n.getPiezasProbadas() == 2) {
								n.setPieza(n.getPiezasProbadas());
								n.setCoste(calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], n.getPieza()));
							}
						}
						
						n.setPiezasProbadas(n.getPiezasProbadas() + 1); /* piezasProbadas++ */

						n.getVisitados().add(n.getPos());
						res.getCamino().add(n);
						if (profundidadActual < cota){ /* Si no hemos llegado a la cota */
							generarCandidatos(n, direccion, profundidadActual,profundidadActual+1);
							profundidadActual++;
						}else{
							/* Cota excedida */
							if (profundidadActual>=2){
								/* Quitamos la ultima pieza colocada y no valida */
								n=res.getCamino().get(profundidadActual);
								if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
								res.getCamino().remove(profundidadActual);
								/* Quitamos el padre porque no ha sido objetivo y esta en la frontera */
								profundidadActual--;
								n=res.getCamino().get(profundidadActual);
								if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
								res.getCamino().remove(profundidadActual);
							}
						}
						
						if (maxNodos < monticulo.size())
							maxNodos = monticulo.size();
						
						if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1; /* Si hay curva se cambia de direccion */
						
					} else { /* Si hemos probado todas las piezas, volvemos atras */
						if (profundidadActual>=2){
							/* Quitamos la ultima pieza colocada y no valida */
							profundidadActual--;
							n=res.getCamino().get(profundidadActual);
							if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
							res.getCamino().remove(profundidadActual);
							/* Quitamos el padre y lo volvemos a incluir como nodo objetivo para que intente seguir otro camino */
							profundidadActual--;
							n=res.getCamino().get(profundidadActual);
							res.getCamino().remove(profundidadActual);
							n.setCoste(0);
							if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
							n.setPieza(0);
							monticulo.add(n);
						}
					}
				}
			}else{ /* Si no hay candidatos */
				if(profundidadActual>=1){
					/* Quitamos el padre y lo volvemos a incluir como nodo objetivo para que intente seguir otro camino */
					profundidadActual--;
					n=res.getCamino().get(profundidadActual);
					res.getCamino().remove(profundidadActual);
					n.setCoste(0);
					if (n.getPieza() == 3 || n.getPieza() == 4) direccion*=-1;
					n.setPieza(0);
					monticulo.add(n);
				}else{
					solucionEncontrada=true;
					res.setM(maxNodos);
					System.out.println("Solucion NO encontrada");
				}
			}
		}

		return res;
	}

	private Camino busquedaProfundidadIterativa(int[][] mapa, Posicion posIni,
			Posicion posFin, long cota, long aumentocota, int numItera) {
		Camino res = new Camino();
		int iteracion=0;
		
		boolean solucionEncontrada=false;
		while (iteracion<numItera && !solucionEncontrada){
			System.out.println("Iteracion " + iteracion + " - Cota " + cota);
			res=busquedaProfundidadAcotada(mapa,posIni,posFin,cota);
			if(res.getCamino().isEmpty()){
				cota+=aumentocota;
				iteracion++;
			}else solucionEncontrada=true;
		}
		return res;
	}

	private Camino busquedaAnchura(int[][] mapa, Posicion posIni, Posicion posFin) {
		Camino resOptimo = new Camino();
		Camino res = new Camino();
		boolean solucionEncontrada = false;

		/* Inicializamos la solucion optima con el peor valor */
		resOptimo.setC(99999999.0);
		/* Anadimos el primer nodo como candidato */
		Nodo n = new Nodo(posIni, 0, 0, 0, 0, null);
		n.setDireccion(1);	/* Escogemos horizontal como la primera direccion */
		monticulo.add(n);
		
		/* Definimos el nodo error. Este indicara que quedan candidatos, pero que no son
		 * de la misma profundidad que la actual y tendremos que aumentarla. Es decir,
		 * se han comprobado todos los nodos de un nivel.
		 */
		Nodo error=new Nodo(new Posicion(-1,-1),0,0,0,0,null);
		
		Nodo [] copiasCandidato = new Nodo[4];
		int profundidadActual = 0;
		double maxNodos = 0;
		/* Comenzamos el bucle de busqueda */
		while (!solucionEncontrada) {
			n = seleccionarCandidatoA(profundidadActual);
			if (n != null) { /* Hay candidatos */
				if (n.equals(error)){ /* Hemos comprobado todos los candidatos de un nivel */
					profundidadActual++; /* Pasamos al nivel siguiente */
				}else{
					if (n.getPos().equals(posFin)) { /* Si es el nodo objetivo */
						/* Ponemos una pieza de avanzar simple y hemos encontrado una solucion */
						n.setPieza(1);
						n.setCoste(n.getPadre().getCoste() + calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], 1));
						
						n.getVisitados().add(posFin);
						res=construirSolucion(n);
						/* Calculamos el coste de la solucion y el numero maximo de nodos almacenados */
						res.setC(n.getCoste());
						res.setM(maxNodos);
						System.out.println(res.toString3());
						/* Comprobamos si es la mejor solucion */
						if (res.getC() < resOptimo.getC())
							resOptimo=res;
						
					} else { /* Si no es el nodo objetivo */
						
						for (int i=1;i<5;i++){ /* Probamos todas las piezas */
							/* Hacemos una copia del candidato para que no se sobrescriban las piezas */
							copiasCandidato[i-1]= new Nodo(n.getPos(),0,0,n.getF(),n.getProfundidad(),n.getPadre());
							copiasCandidato[i-1].setDireccion(n.getDireccion());
							if(copiasCandidato[i-1].getPadre() != null){  /* Pixel intermedio */
								copiasCandidato[i-1].setPieza(i);
								copiasCandidato[i-1].setCoste(copiasCandidato[i-1].getPadre().getCoste()+ calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
							} else { /* Pixel inicial */
								if(i==1 || i==2){
									copiasCandidato[i-1].setPieza(i);
									copiasCandidato[i-1].setCoste(calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
								}
							}
							copiasCandidato[i-1].getVisitados().add(copiasCandidato[i-1].getPos());
							/* Si el coste aun es menor que el de la mejor solucion */
							if (copiasCandidato[i-1].getCoste()<resOptimo.getC())
								generarCandidatos(copiasCandidato[i-1], copiasCandidato[i-1].getDireccion(), profundidadActual,profundidadActual+1);
							else /* Si no */
								/* Se abandona el camino */;
						}
						if (maxNodos < monticulo.size())
							maxNodos = monticulo.size();
					}
				}
			}else{ /* Si no hay candidatos */
				solucionEncontrada=true;
				if (!resOptimo.getCamino().isEmpty())
					System.out.println("Se ha encontrado Solucion");
				else
					System.out.println("No se ha encontrado Solucion");
			}
		}

		return resOptimo;
	}
	
	private Camino busquedaCostoUniforme(int[][] mapa, Posicion posIni, Posicion posFin) {
		Camino resOptimo = new Camino();
		Camino res = new Camino();
		boolean solucionEncontrada = false;

		/* Inicializamos la solucion optima con el peor valor */
		resOptimo.setC(99999999.0);
		/* Anadimos el primer nodo como candidato */
		Nodo n = new Nodo(posIni, 0, 0, 0, 0, null);
		n.setDireccion(1);	/* Escogemos horizontal como la primera direccion */
		monticulo.add(n);
		
		/* Definimos el nodo error. Este indicara que quedan candidatos, pero que no son
		 * de la misma profundidad que la actual y tendremos que aumentarla. Es decir,
		 * se han comprobado todos los nodos de un nivel.
		 */
		Nodo error=new Nodo(new Posicion(-1,-1),0,0,0,0,null);
		
		Nodo [] copiasCandidato = new Nodo[4];
		int profundidadActual = 0;
		double maxNodos = 0;
		/* Comenzamos el bucle de busqueda */
		while (!solucionEncontrada) {
			n = seleccionarCandidatoP();
			if (n != null) { /* Hay candidatos */
				if (n.equals(error)){ /* Hemos comprobado todos los candidatos de un nivel */
					profundidadActual++; /* Pasamos al nivel siguiente */
				}else{
					if (n.getPos().equals(posFin)) { /* Si es el nodo objetivo */
						/* Ponemos una pieza de avanzar simple y hemos encontrado una solucion */
						n.setPieza(1);
						n.setCoste(n.getPadre().getCoste() + calcularCoste(mapa[(int) n.getPos().getX()][(int) n.getPos().getY()], 1));
						res=construirSolucion(n);
						n.getVisitados().add(posFin);
						/* Calculamos el coste de la solucion y el numero maximo de nodos almacenados */
						res.setC(n.getCoste());
						res.setM(maxNodos);
						System.out.println(res.toString3());
						/* Comprobamos si es la mejor solucion */
						if (res.getC() < resOptimo.getC())
							resOptimo=res;
					} else { /* Si no es el nodo objetivo */
						
						for (int i=1;i<5;i++){ /* Probamos todas las piezas */
							/* Hacemos una copia del candidato para que no se sobrescriban las piezas */
							copiasCandidato[i-1]= new Nodo(n.getPos(),0,0,n.getF(),n.getProfundidad(),n.getPadre());
							copiasCandidato[i-1].setDireccion(n.getDireccion());
							if(copiasCandidato[i-1].getPadre() != null){  /* Pixel intermedio */
								copiasCandidato[i-1].setPieza(i);
								copiasCandidato[i-1].setCoste(copiasCandidato[i-1].getPadre().getCoste()+ calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
							} else { /* Pixel inicial */
								if(i==1 || i==2){
									copiasCandidato[i-1].setPieza(i);
									copiasCandidato[i-1].setCoste(calcularCoste(mapa[(int) copiasCandidato[i-1].getPos().getX()][(int) copiasCandidato[i-1].getPos().getY()], copiasCandidato[i-1].getPieza()));
								}
							}
							copiasCandidato[i-1].getVisitados().add(copiasCandidato[i-1].getPos());
							/* Si el coste aun es menor que el de la mejor solucion */
							if (copiasCandidato[i-1].getCoste()<resOptimo.getC())
								generarCandidatos(copiasCandidato[i-1], copiasCandidato[i-1].getDireccion(),
										profundidadActual, copiasCandidato[i-1].getCoste());
										/* Como es busqueda con costo uniforme, le pasamos el coste de colocar
										 * la pieza en el valor que tendra F en los hijos */
							else /* Si no */
								/* Se abandona este camino*/ ;
						}
						if (maxNodos < monticulo.size())
							maxNodos = monticulo.size();
					}
				}
			}else{ /* Si no hay candidatos */
				solucionEncontrada=true;
				if (!resOptimo.getCamino().isEmpty())
					System.out.println("Se ha encontrado Solucion");
				else
					System.out.println("No se ha encontrado Solucion");
			}
		}

		return resOptimo;
	}
	
	private void generarCandidatos(Nodo n, int direccion, int profundidadActual, double f) {
		Nodo candidato;
		Posicion p=new Posicion(-1, -1);
		int sentido=-500; /* En caso de que venga un nodo sin direccion se descarta sacandolo de la imagen */
		/* Si la direccion es horizontal */
		if (direccion==1){
			if (n.getPadre()!=null){
				if(n.getPos().getX()<n.getPadre().getPos().getX())
					sentido=-1;
				else if (n.getPos().getX()>n.getPadre().getPos().getX())
					sentido=1;
			}else sentido=1; /* Nodo inicial, direccion horizontal, sentido dcha */
			switch (n.getPieza()){
				case 1:
					p = new Posicion(n.getPos().getX() + 1 * sentido, n.getPos().getY());
					break;
				case 2:
					p = new Posicion(n.getPos().getX() + 2 * sentido, n.getPos().getY());
					break;
				case 3:
					p = new Posicion(n.getPos().getX(), n.getPos().getY() + 1 * sentido);
					break;
				case 4:
					p = new Posicion(n.getPos().getX(), n.getPos().getY() - 1 * sentido);
					break;
				default:
					/* Sin Pieza */ ;
					break;
			}
		}
		
		/* Si la direccion es vertical */
		if (direccion==-1){
			if(n.getPadre()!=null){
				if (n.getPos().getY()<n.getPadre().getPos().getY())
					sentido=-1;
				else if (n.getPos().getY()>n.getPadre().getPos().getY())
					sentido=1;
			} else sentido = 1; /* Nodo inicial, direccion vertical, sentido abajo */
			
			switch (n.getPieza()){
				case 1:
					p = new Posicion(n.getPos().getX(), n.getPos().getY() + 1 * sentido);
					break;
				case 2:
					p = new Posicion(n.getPos().getX(), n.getPos().getY() + 2 * sentido);
					break;
				case 3:
					p = new Posicion(n.getPos().getX() + 1 * sentido, n.getPos().getY());
					break;
				case 4:
					p = new Posicion(n.getPos().getX() - 1 * sentido, n.getPos().getY());
					break;
			}
		}
	
		
		if (p.getX()<width &&  p.getX()>=0 && p.getY()<height && p.getY()>=0){
			/* Si esta dentro de la imagen */
			candidato=new Nodo(p,0,0,f,profundidadActual+1,n);
			if (n.getPieza() == 3 || n.getPieza() == 4) candidato.setDireccion(n.getDireccion()*-1); /* Si hay curva se cambia de direccion */
			else candidato.setDireccion(n.getDireccion());
			
			/* Si no esta visitado */
			if (!n.getVisitados().contains(p))
				monticulo.add(candidato);
			else /* Posicion ya visitada */;
		}
	}
}
